#include "stdafx.h"
#include <math.h>
#include "Matrix.h"

using namespace aiRobots;

Matrix::Matrix()
{
	this->Renew(3, 3);
}

Matrix::Matrix(int row, int col)
{
	this->Renew(row, col);
}

Matrix::Matrix(int row, int col, mtype value)
{
	this->Renew(row, col, value);
}

Matrix::Matrix(int row, int col, mtype* value)
{
	this->Renew(row, col, value);
}

void Matrix::Renew(int row, int col)
{
	this->RowNum = row;
	this->ColNum = col;
	this->Length = this->RowNum * this->ColNum;

	if(this->Length < 1)
	{
		this->RowNum = 1;
		this->ColNum = 1;
		this->Length = 1;
	}
	else if(this->Length > MSIZE)
	{
		this->RowNum = 4;
		this->ColNum = 4;
		this->Length = 16;
	}
	for(int i=0; i < this->Length; i++)
	{
		this->Value[i] = 0;
	}
}

void Matrix::Renew(int row, int col, mtype value)
{
	this->RowNum = row;
	this->ColNum = col;
	this->Length = this->RowNum * this->ColNum;

	if(this->Length < 1)
	{
		this->RowNum = 1;
		this->ColNum = 1;
		this->Length = 1;
	}
	else if(this->Length > MSIZE)
	{
		this->RowNum = 4;
		this->ColNum = 4;
		this->Length = 16;
	}
	for(int i=0; i < this->Length; i++)
	{
		this->Value[i] = value;
	}
}

void Matrix::Renew(int row, int col, mtype* value)
{
	this->RowNum = row;
	this->ColNum = col;
	this->Length = this->RowNum * this->ColNum;

	if(this->Length < 1)
	{
		this->RowNum = 1;
		this->ColNum = 1;
		this->Length = 1;
	}
	else if(this->Length > MSIZE)
	{
		this->RowNum = 4;
		this->ColNum = 4;
		this->Length = 16;
	}
	for(int i=0; i < this->Length; i++)
	{
		this->Value[i] = value[i];
	}
}

void Matrix::SetDiagonal(mtype value)
{
	int temp = 0;
	for(int i=0; i < this->RowNum; i++)
	{
		for(int j=0; j < this->ColNum; j++)
		{
			temp = (i * this->ColNum) + j;
			if(i == j)
			{
				this->Value[temp] = value;
			}
			else
			{
				this->Value[temp] = 0;
			}
		}
	}
}

void Matrix::SetValue(mtype value)
{
	for(int i=0; i < this->Length; i++)
	{
		this->Value[i] = value;
	}
}

void Matrix::SetValue(int row, int col, mtype value)
{
	if((row < this->RowNum) && (col < this->ColNum))
	{
		int i = (row * this->ColNum) + col;
		this->Value[i] = value;
	}
}

void Matrix::SetValue(mtype* value)
{
	int temp = 0;
	for(int i=0; i < this->RowNum; i++)
	{
		for(int j=0; j < this->ColNum; j++)
		{
			temp = (i * this->ColNum) + j;
			this->Value[temp] = value[temp];
		}
	}
}

void Matrix::GetValue(mtype* value)
{
	int temp = 0;
	for(int i=0; i < this->RowNum; i++)
	{
		for(int j=0; j < this->ColNum; j++)
		{
			temp = (i * this->ColNum) + j;
			value[temp] = this->Value[temp];
		}
	}
}

mtype Matrix::GetValue(int row, int col)
{
	if((row < this->RowNum) && (col < this->ColNum))
	{
		int temp = (row * this->ColNum) + col;
		return this->Value[temp];
	}
	return 0;
}

int Matrix::GetRowNum()
{
	return this->RowNum;
}

int Matrix::GetColNum()
{
	return this->ColNum;
}

Matrix Matrix::Transpose()
{
	int temp1, temp2;
	Matrix result(ColNum, RowNum);

	for(int i=0; i < RowNum; i++)
	{
		for(int j=0; j < ColNum; j++)
		{
			temp1 = i * ColNum + j;
			temp2 = j * RowNum + i;
			result.Value[temp2] = Value[temp1];
		}
	}
	return result;
}

bool Matrix::Inverse(Matrix &destination)
{
	if(RowNum == 2 && ColNum == 2)
	{
		mtype temp[4] = {0};
		this->GetValue(&temp[0]);
		mtype Det = temp[0]*temp[3] - temp[1]*temp[2];
		if(Det != 0)
		{

			destination.SetValue(0, 0, temp[3] / Det);
			destination.SetValue(0, 1, -temp[1] / Det);
			destination.SetValue(1, 0, -temp[2] / Det);
			destination.SetValue(1, 1, temp[0] / Det);
			return true;
		}
	}
	else if(RowNum == 3 && ColNum == 3)
	{
		mtype temp[9] = {0};
		this->GetValue(&temp[0]);
		mtype Det = temp[0]*temp[4]*temp[8] + temp[1]*temp[5]*temp[6] +
					temp[2]*temp[3]*temp[7] - temp[2]*temp[4]*temp[6] -
					temp[0]*temp[5]*temp[7] - temp[1]*temp[3]*temp[8];
		if(Det != 0)
		{
			destination.SetValue(0, 0, (temp[4]*temp[8] - temp[5]*temp[7]) / Det);
			destination.SetValue(0, 1, -(temp[1]*temp[8] - temp[7]*temp[2]) / Det);
			destination.SetValue(0, 2, (temp[1]*temp[5] - temp[4]*temp[2]) / Det);
			destination.SetValue(1, 0, -(temp[3]*temp[8] - temp[6]*temp[5]) / Det);
			destination.SetValue(1, 1, (temp[0]*temp[8] - temp[6]*temp[2]) / Det);
			destination.SetValue(1, 2, -(temp[0]*temp[5] - temp[3]*temp[2]) / Det);
			destination.SetValue(2, 0, (temp[3]*temp[7] - temp[6]*temp[4]) / Det);
			destination.SetValue(2, 1, -(temp[0]*temp[7] - temp[6]*temp[1]) / Det);
			destination.SetValue(2, 2, (temp[0]*temp[4] - temp[1]*temp[3]) / Det);
			return true;
		}
	}
	return false;
}

void Matrix::Print()
{
	for(int i=0; i < RowNum; i++)
	{
		for (int j=0 ; j < ColNum; j++)
		{
			cout << (mtype)((int)(Value[(i * this->ColNum) + j]*10000)) / 10000 << ", ";
		}
		cout << endl;
	}
}

Matrix Matrix::operator=(const Matrix &m)
{
	RowNum = m.RowNum;
	ColNum = m.ColNum;
	Length = m.Length;
	for(int i=0; i < Length; i++)
	{
		Value[i] = m.Value[i];
	}
	return *this;
}

Matrix Matrix::operator+(const Matrix &m)
{
	Matrix result(RowNum, ColNum, &Value[0]);
	if(Length == m.Length)
	{
		for(int i=0; i < Length; i++)
		{
			result.Value[i] = Value[i] + m.Value[i];
		}
	}
	return result;
}

Matrix Matrix::operator-(const Matrix &m)
{
	Matrix result(RowNum, ColNum, &Value[0]);
	if(Length == m.Length)
	{
		for(int i=0; i < Length; i++)
		{
			result.Value[i] = Value[i] - m.Value[i];
		}
	}
	return result;
}

Matrix Matrix::operator*(const Matrix &m)
{
	int temp = 0;
	mtype sum = 0;
	Matrix result(RowNum, m.ColNum);
	if(ColNum == m.RowNum)
	{
		for(int i=0; i < RowNum; i++)
		{
			for(int j=0; j < m.ColNum; j++)
			{
				temp = i * m.ColNum + j;
				result.Value[temp] = 0;
				for(int k=0; k < RowNum; k++)
				{
					result.Value[temp] += Value[(i*ColNum)+k] * m.Value[(k*m.ColNum)+j];
				}
			}
		}
	}
	return result;
}

Matrix Matrix::operator*(mtype a)
{
	Matrix result(RowNum, ColNum, &Value[0]);
	for(int i=0; i < Length; i++)
	{
		result.Value[i] = Value[i] * a;
	}
	return result;
}

Matrix Matrix::operator/(mtype a)
{
	Matrix result(RowNum, ColNum, &Value[0]);
	if(a != 0)
	{
		for(int i=0; i < Length; i++)
		{
			result.Value[i] = Value[i] / a;
		}
	}
	return result;
}
